﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Windows.Media.Media3D;
using LFSRecord2.Misc;
using LFSRecord2.Structures;
using LFSRecord2.Model.UserActions;
using System.Collections.ObjectModel;

namespace LFSRecord2.Model
{
    public static class CustomPoint3DInterpolation
    {
        public static Point3DFrameCacheObject[] createLinearSection(CustomPoint3DKeyFrame fromKey, CustomPoint3DKeyFrame toKey)
        {
            uint numFrames = (uint)(toKey.FrameNum - fromKey.FrameNum);
            Point3DFrameCacheObject[] sectionPoints = new Point3DFrameCacheObject[numFrames];
            Point3DFrameCacheObject fo;

            double timeMult = 1.0d / numFrames;
            for (int a = 0; a < numFrames; a++)
            {
                fo = new Point3DFrameCacheObject();
                fo.Value = InterpolateLinear(fromKey, toKey, CustomDoubleEase.EasingCore(a * timeMult, fromKey.EaseOutValue, toKey.EaseInValue));

                sectionPoints[a] = fo;
            }

            return sectionPoints;
        }

        public static Point3D InterpolateLinear(Point3D fromPoint, Point3D toPoint, double normalizedTime)
        {
            Point3D p = new Point3D();
            p.X = normalizedTime * (toPoint.X - fromPoint.X) + fromPoint.X;
            p.Y = normalizedTime * (toPoint.Y - fromPoint.Y) + fromPoint.Y;
            p.Z = normalizedTime * (toPoint.Z - fromPoint.Z) + fromPoint.Z;
            return p;
        }

        public static Point3D InterpolateLinear(CustomPoint3DKeyFrame fromKey, CustomPoint3DKeyFrame toKey, double normalizedTime)
        {
            Point3D p = new Point3D();
            p.X = normalizedTime * (toKey.Value.X - fromKey.Value.X) + fromKey.Value.X;
            p.Y = normalizedTime * (toKey.Value.Y - fromKey.Value.Y) + fromKey.Value.Y;
            p.Z = normalizedTime * (toKey.Value.Z - fromKey.Value.Z) + fromKey.Value.Z;
            return p;
        }

        public static Point3D InterpolateLinear(CustomPoint3DKeyFrameHelper fromKey, CustomPoint3DKeyFrameHelper toKey, double timelineIndex)
        {
            // Normalize the time
            if (fromKey.NormalizedTime == timelineIndex)
                return fromKey.Value;

            // Normalize the time values
            timelineIndex -= fromKey.NormalizedTime;
            double endTime = toKey.NormalizedTime - fromKey.NormalizedTime;

            // Easing
            //double normalizedTime = toKey.Easing.EaseInCore((double)timelineIndex / endTime);
            double normalizedTime = timelineIndex / endTime;

            // Interpolate
            Point3D p = new Point3D();
            p.X = normalizedTime * (toKey.Value.X - fromKey.Value.X) + fromKey.Value.X;
            p.Y = normalizedTime * (toKey.Value.Y - fromKey.Value.Y) + fromKey.Value.Y;
            p.Z = normalizedTime * (toKey.Value.Z - fromKey.Value.Z) + fromKey.Value.Z;
            return p;
        }

        public static Point3DFrameCacheObject[] createBezierSection(CustomPoint3DKeyFrame fromKey, CustomPoint3DKeyFrame toKey)
        {
            uint numFrames = (uint)(toKey.FrameNum - fromKey.FrameNum);

            InterpolateCacheHelper bezierPath = getNormalizedBezierPath(fromKey, toKey);
            Point3DFrameCacheObject[] sectionPoints = new Point3DFrameCacheObject[numFrames];

            //Debug.WriteLine("Creating cached bezier section - " + timeDiff + " - " + numFrames);

            double timeMult = 1.0d / (double)numFrames;
            int startKeyFrame = 0;
            double continueBezierTime = 0;
            for (int a = 0; a < numFrames; a++)
            {
                //Debug.WriteLine("Time {0} - EaseCoreValue : {1}", a * timeMult, CustomDoubleEase.EasingCore(a * timeMult, fromKey.EaseOutValue, toKey.EaseInValue));
                sectionPoints[a] = bezierPath.GetValueAtTimeWithBezierTime(
                    CustomDoubleEase.EasingCore(a * timeMult, fromKey.EaseOutValue, toKey.EaseInValue), 
                    ref startKeyFrame, 
                    ref continueBezierTime);
            }

            return sectionPoints;
        }

        public static void writeHandlesForInsertedBezierPoint(CustomPoint3DKeyFrame kf, CustomPoint3DKeyFrame kfPrev, CustomPoint3DKeyFrame kfNext, Collection<Point3DFrameCacheObject> pointCache, int firstFrame)
        {
            int curFrame = kf.FrameNum - firstFrame;
            //Debug.WriteLine("BezierTime at new keyframe ("+curFrame+") : " + pointCache[curFrame].BezierTime);

            double bezierTime = (curFrame > pointCache.Count - 1) ? 0 : pointCache[curFrame].BezierTime;

            // Calculate first new control point
            Point3D p1 = new Point3D(
                ((kfNext.Value.X + kfNext.BezierControlIn.X) - (kfPrev.Value.X + kfPrev.BezierControlOut.X)) *
                    bezierTime + (kfPrev.Value.X + kfPrev.BezierControlOut.X),
                ((kfNext.Value.Y + kfNext.BezierControlIn.Y) - (kfPrev.Value.Y + kfPrev.BezierControlOut.Y)) *
                    bezierTime + (kfPrev.Value.Y + kfPrev.BezierControlOut.Y),
                ((kfNext.Value.Z + kfNext.BezierControlIn.Z) - (kfPrev.Value.Z + kfPrev.BezierControlOut.Z)) *
                    bezierTime + (kfPrev.Value.Z + kfPrev.BezierControlOut.Z)
                );

            // Set neighbours' bezier controls
            Point3D bezierOutOrig = kfPrev.BezierControlOut;
            kfPrev.BezierControlOut = new Point3D(
                kfPrev.BezierControlOut.X * bezierTime,
                kfPrev.BezierControlOut.Y * bezierTime,
                kfPrev.BezierControlOut.Z * bezierTime
                );
            LfsRecordController.RecordUserAction(new UserActionKeyFrameBezierChanged()
                {
                    LayerIndex = kfPrev.ParentLayer.LayerIndex,
                    PropertyIndex = kfPrev.ParentProperty.PropertyIndex,
                    KeyFrameIndex = kfPrev.KeyFrameIndex,
                    BezierControlInOffset = new Vector3D(),
                    BezierControlOutOffset = Point3D.Subtract(kfPrev.BezierControlOut, bezierOutOrig)
                });

            Point3D bezierInOrig = kfNext.BezierControlIn;
            kfNext.BezierControlIn = new Point3D(
                kfNext.BezierControlIn.X * (1 - bezierTime),
                kfNext.BezierControlIn.Y * (1 - bezierTime),
                kfNext.BezierControlIn.Z * (1 - bezierTime)
                );
            LfsRecordController.RecordUserAction(new UserActionKeyFrameBezierChanged()
            {
                LayerIndex = kfNext.ParentLayer.LayerIndex,
                PropertyIndex = kfNext.ParentProperty.PropertyIndex,
                KeyFrameIndex = kfNext.KeyFrameIndex,
                BezierControlInOffset = Point3D.Subtract(kfNext.BezierControlIn, bezierInOrig),
                BezierControlOutOffset = new Vector3D()
            });

            // Switch off neighbouring keyframes' autobezier. Also record these as user actions, if they changed.
            CustomKeyFrameOptions oldOptions;
            if ((kfPrev.Options & CustomKeyFrameOptions.AutoBezier) > 0)
            {
                oldOptions = kfPrev.Options;
                kfPrev.Options &= ~CustomKeyFrameOptions.AutoBezier;
                LfsRecordController.RecordUserAction(new UserActionKeyFrameOptionChanged()
                    {
                        LayerIndex = kfPrev.ParentLayer.LayerIndex,
                        PropertyIndex = kfPrev.ParentProperty.PropertyIndex,
                        KeyFrameIndex = kfPrev.KeyFrameIndex,
                        OldOptions = oldOptions,
                        NewOptions = kfPrev.Options
                    });
            }
            if ((kfNext.Options & CustomKeyFrameOptions.AutoBezier) > 0)
            {
                oldOptions = kfNext.Options;
                kfNext.Options &= ~CustomKeyFrameOptions.AutoBezier;
                LfsRecordController.RecordUserAction(new UserActionKeyFrameOptionChanged()
                    {
                        LayerIndex = kfNext.ParentLayer.LayerIndex,
                        PropertyIndex = kfNext.ParentProperty.PropertyIndex,
                        KeyFrameIndex = kfNext.KeyFrameIndex,
                        OldOptions = oldOptions,
                        NewOptions = kfNext.Options
                    });
            }
            //LfsRecordController.CreateUndoLevel();

            // Control point 0
            Point3D p0 = new Point3D(
                kfPrev.Value.X + kfPrev.BezierControlOut.X,
                kfPrev.Value.Y + kfPrev.BezierControlOut.Y,
                kfPrev.Value.Z + kfPrev.BezierControlOut.Z
                );
            // Control point 2
            Point3D p2 = new Point3D(
                kfNext.Value.X + kfNext.BezierControlIn.X,
                kfNext.Value.Y + kfNext.BezierControlIn.Y,
                kfNext.Value.Z + kfNext.BezierControlIn.Z
                );

            kf.BezierControlIn = new Point3D(
                ((p1.X - p0.X) * bezierTime + p0.X) - kf.Value.X,
                ((p1.Y - p0.Y) * bezierTime + p0.Y) - kf.Value.Y,
                ((p1.Z - p0.Z) * bezierTime + p0.Z) - kf.Value.Z
                );
            kf.BezierControlOut = new Point3D(
                ((p2.X - p1.X) * bezierTime + p1.X) - kf.Value.X,
                ((p2.Y - p1.Y) * bezierTime + p1.Y) - kf.Value.Y,
                ((p2.Z - p1.Z) * bezierTime + p1.Z) - kf.Value.Z
                );

            kf.Options = kf.Options & ~CustomKeyFrameOptions.AutoBezier;
        }

        public static InterpolateCacheHelper getNormalizedBezierPath(CustomPoint3DKeyFrame fromKey, CustomPoint3DKeyFrame toKey)
        {
            Point3D fromOutCP;
            if (fromKey.InterpolationTypeOut == KeyFrameInterpolationTypes.Bezier)
                fromOutCP = new Point3D(fromKey.Value.X + fromKey.BezierControlOut.X,
                                        fromKey.Value.Y + fromKey.BezierControlOut.Y,
                                        fromKey.Value.Z + fromKey.BezierControlOut.Z);
            else
                fromOutCP = new Point3D(fromKey.Value.X,
                                        fromKey.Value.Y,
                                        fromKey.Value.Z);
            Point3D toInCP;
            if (toKey.InterpolationTypeIn == KeyFrameInterpolationTypes.Bezier)
                toInCP = new Point3D(toKey.Value.X + toKey.BezierControlIn.X,
                                     toKey.Value.Y + toKey.BezierControlIn.Y,
                                     toKey.Value.Z + toKey.BezierControlIn.Z);
            else
                toInCP = new Point3D(toKey.Value.X,
                                     toKey.Value.Y,
                                     toKey.Value.Z);

            int a;
            int subFrameMult = 1;
            double numSubFrames = toKey.FrameNum - fromKey.FrameNum;
            if (numSubFrames < 10)
            {
                subFrameMult = 5;
                numSubFrames *= subFrameMult;
            }
            else if (numSubFrames < 20)
            {
                subFrameMult = 2;
                numSubFrames *= subFrameMult;
            }
            double subFrameTime = 1.0d / numSubFrames;

            // Create the bezier curve
            double t, val0, val1, val2, val3;
            Point3D[] bezierCurve = new Point3D[(uint)(numSubFrames + 1)];
            bezierCurve[0] = new Point3D(fromKey.Value.X, fromKey.Value.Y, fromKey.Value.Z);
            for (a = 1; a < numSubFrames; a++)
            {
                t = a / numSubFrames;

                val0 = Math.Pow(1 - t, 3);
                val1 = 3 * Math.Pow(1 - t, 2) * t;
                val2 = 3 * (1 - t) * Math.Pow(t, 2);
                val3 = Math.Pow(t, 3);

                bezierCurve[a] = new Point3D(
                    val0 * fromKey.Value.X +
                    val1 * fromOutCP.X +
                    val2 * toInCP.X +
                    val3 * toKey.Value.X
                    ,
                    val0 * fromKey.Value.Y +
                    val1 * fromOutCP.Y +
                    val2 * toInCP.Y +
                    val3 * toKey.Value.Y
                    ,
                    val0 * fromKey.Value.Z +
                    val1 * fromOutCP.Z +
                    val2 * toInCP.Z +
                    val3 * toKey.Value.Z
                );
            }
            bezierCurve[(uint)numSubFrames] = new Point3D(toKey.Value.X, toKey.Value.Y, toKey.Value.Z);

            // Calculate the length of the curve and of each section
            double bezierLength = 0;
            double[] bezierSectionLengths = new double[(uint)numSubFrames];
            for (a = 0; a < numSubFrames; a++)
            {
                bezierSectionLengths[a] = MathHelper.PointDistance(bezierCurve[a], bezierCurve[a + 1]);
                bezierLength += bezierSectionLengths[a];
                //Debug.WriteLine("Section length : " + bezierSectionLengths[a] + " {0},{1},{2} - {3},{4},{5}",
                //    bezierCurve[a].X, bezierCurve[a].Y, bezierCurve[a].Z,
                //    bezierCurve[a + 1].X, bezierCurve[a + 1].Y, bezierCurve[a + 1].Z);
                //Debug.WriteLine("Distance : " + bezierSectionLengths[a]);
            }

            // Here we have the opportunity to set the fromKey's real DistanceToNextKeyFrame
            fromKey.DistanceToNextKeyFrame = bezierLength;

            // average speed per frame (average distance per frame)
            double averageLength = (bezierLength / numSubFrames);
            //Debug.WriteLine("Average : " + averageLength);

            // Populate timeline with paced keyframes
            double normalizedTime, curLength = 0, lengthDiff, sectionBezierTime, curBezierTime = 0, bezierTimeDiff;
            int subFrameCounter = 1, frameCounter = 1;
            Point3D p;
            InterpolateCacheHelper bezierAnimation = new InterpolateCacheHelper();
            bezierAnimation.KeyFrames.Add(new CustomPoint3DKeyFrameHelper(0, bezierCurve[0]));

            if (averageLength == 0)
            {
                for (a = 1; a <= numSubFrames; a++)
                {
                    normalizedTime = a / numSubFrames;
                    bezierAnimation.KeyFrames.Add(new CustomPoint3DKeyFrameHelper(normalizedTime, bezierCurve[a]));
                    bezierAnimation.KeyFrames[a].BezierTime = normalizedTime;
                }
            }
            else
            {
                for (a = 1; a <= numSubFrames; a++)
                {
                    curLength += bezierSectionLengths[a - 1];
                    curBezierTime += subFrameTime;
                    while (curLength >= averageLength)
                    {
                        lengthDiff = curLength - averageLength;
                        bezierTimeDiff = curBezierTime - curBezierTime * (averageLength / curLength);
                        normalizedTime = (bezierSectionLengths[a - 1] - lengthDiff) / bezierSectionLengths[a - 1];

                        // Do a linear interpolation between previous and this point
                        p = InterpolateLinear(bezierCurve[a - 1], bezierCurve[a], normalizedTime);
                        sectionBezierTime = curBezierTime - bezierTimeDiff;
                        //Debug.WriteLine("Distance : " + sectionBezierTime);

                        // Add keyframe
                        if (subFrameMult == 1 || (subFrameCounter % subFrameMult) == 0)
                        {
                            bezierAnimation.KeyFrames.Add(new CustomPoint3DKeyFrameHelper((subFrameCounter / numSubFrames), p));
                            bezierAnimation.KeyFrames[frameCounter].BezierTime = sectionBezierTime;
                            frameCounter++;
                        }
                        subFrameCounter++;

                        curLength = lengthDiff;
                        curBezierTime = bezierTimeDiff;
                    }
                }
            }
            if (bezierAnimation.KeyFrames.Count < numSubFrames + 1)
            {
                bezierAnimation.KeyFrames.Add(new CustomPoint3DKeyFrameHelper(1, toKey.Value));
            }

            //Debug.WriteLine("Bezier subFrames : " + numSubFrames + " (" + bezierAnimation.KeyFrames.Count + ") ("+frameCounter+") - remainder : " + curLength + " - total len : " + bezierLength + " - ave len : " + averageLength);
            return bezierAnimation;
        }
    }
}
